using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;

namespace Graphs
{

	enum CNodeRole
	{
		Any,
		Start,
		End

	}

	class CGraph
	{

		private RectangleF bounds = new RectangleF((float) Double.PositiveInfinity, (float) Double.PositiveInfinity, 0, 0);

		public RectangleF Bounds
		{
			get { return bounds; }
		}



		private CNodeList nodeList;
		public CNodeList Nodes
		{
			get { return nodeList; }
		}


		private CEdgeList edgeList;
		public CEdgeList Edges
		{
			get { return edgeList; }
		}


		public CGraph()
		{
			nodeList = new CNodeList();
			edgeList = new CEdgeList();
		}


		public int AddNode(CNode newNode)
		{
			if (nodeList.Contains(newNode))
			{
				return nodeList.IndexOf(newNode);
			}
			nodeList.Add(newNode);
			bounds.X = Math.Min(bounds.X, newNode.Location.X);
			bounds.Y = Math.Min(bounds.Y, newNode.Location.Y);
			bounds.Width = Math.Max(bounds.Width, newNode.Location.X - bounds.X);
			bounds.Height = Math.Max(bounds.Height, newNode.Location.Y - bounds.Y);

			return nodeList.IndexOf(newNode);
		}

		public bool RemoveNode(CNode node)
		{
			return nodeList.Remove(node);
		}

		public CNode GetNode(int index)
		{
			if (nodeList.Count > index)
				return nodeList [index];
			return null;
		}

		public CNode GetNode(string name)
		{
			foreach (CNode node in nodeList)
			{
				if (node.Name == name)
				{
					return node;
				}
			}
			return null;
		}
		public CNode GetNode(PointF p)
		{
			foreach (CNode node in nodeList)
			{
				if (node.Location == p)
				{
					return node;
				}
			}
			return null;
		}

		public int AddEdge(CEdge newEdge)
		{
			if (edgeList.Contains(newEdge))
			{
				return edgeList.IndexOf(newEdge);
			}
			edgeList.Add(newEdge);
			return edgeList.IndexOf(newEdge);
		}

		public int AddEdge(CNode startNode, CNode endNode)
		{
			AddNode(startNode);
			AddNode(endNode);
			CEdge newEdge = new CEdge(startNode, endNode);
			edgeList.Add(newEdge);
			return edgeList.IndexOf(newEdge);
		}

		public bool RemoveEdge(CEdge edge)
		{
			return RemoveEdge(edge, false);
		}

		public bool RemoveEdge(CEdge edge, bool removeNodes)
		{
			if (removeNodes)
			{
				return RemoveEdges(edge.StartNode) || RemoveEdges(edge.EndNode) || RemoveNode(edge.StartNode) || RemoveNode(edge.EndNode);
			}
			else
			{
				return edgeList.Remove(edge);
			}

		}

		public bool RemoveEdge(CNode startNode, CNode endNode)
		{
			return edgeList.Remove(GetEdge(startNode, endNode));
		}

		public bool RemoveEdges(CNode includedNode)
		{
			bool ret = false;
			List<CEdge> tmp = new List<CEdge>();

			foreach (CEdge edge in edgeList)
			{
				if (edge.StartNode == includedNode || edge.EndNode == includedNode)
				{
					if (!tmp.Contains(edge))
					{
						tmp.Add(edge);
					}
				}
			}

			foreach (CEdge edge in tmp)
			{
				ret |= edgeList.Remove(edge);
			}

			return ret;
		}



		public CEdge GetEdge(CNode startNode, CNode endNode, bool anyRole)
		{

			foreach (CEdge edge in edgeList)
			{

				if (anyRole && (edge.StartNode == startNode && edge.EndNode == endNode) || edge.StartNode == endNode && edge.EndNode == startNode)
				{
					return edge;
				}
				else
					if (edge.StartNode == startNode && edge.EndNode == endNode)
					{
						return edge;
					}
			}
			return null;
		}


		public CEdge GetEdge(CNode startNode, CNode endNode)
		{
			return GetEdge(startNode, endNode, false);
		}


		public CEdge GetEdge(int index)
		{
			if (edgeList.Count > index)
				return edgeList [index];
			return null;
		}



		public List<CEdge> GetEdges(CNode includedNode)
		{
			return GetEdges(includedNode, CNodeRole.Any);

		}


		public List<CEdge> GetEdges(CNode includedNode, CNodeRole nodeRole)
		{

			List<CEdge> tmp = new List<CEdge>();


			foreach (CEdge edge in edgeList)
			{
				bool cond = false;
				switch (nodeRole)
				{
					case CNodeRole.Any:
						cond = (edge.StartNode == includedNode || edge.EndNode == includedNode);
						break;
					case CNodeRole.Start:
						cond = (edge.StartNode == includedNode);
						break;
					case CNodeRole.End:
						cond = (edge.EndNode == includedNode);
						break;
				}

				//if (edge.StartNode == includedNode || edge.EndNode == includedNode)
				if (cond)
				{
					if (!tmp.Contains(edge))
					{
						tmp.Add(edge);
					}
				}
			}


			return tmp;
		}



		public List<CNode> GetAdjacent(CNode node)
		{
			return GetAdjacent(node, CNodeRole.Any);

		}

		public List<CNode> GetAdjacent(CNode node, CNodeRole nodeRole)
		{
			List<CNode> ret = new List<CNode>();

			List<CEdge> eds = GetEdges(node, nodeRole);
			foreach (CEdge edge in eds)
			{
				switch (nodeRole)
				{
					case CNodeRole.Any:
						if (edge.StartNode != node && !ret.Contains(edge.StartNode))
							ret.Add(edge.StartNode);
						if (edge.EndNode != node && !ret.Contains(edge.EndNode))
							ret.Add(edge.EndNode);
						break;
					case CNodeRole.Start:
						if (edge.StartNode != node && !ret.Contains(edge.StartNode))
							ret.Add(edge.StartNode);
						break;
					case CNodeRole.End:
						if (edge.EndNode != node && !ret.Contains(edge.EndNode))
							ret.Add(edge.EndNode);
						break;
				}

			}
			return ret;
		}
















		public void SearchPath(CNode start, CNode end)
		{
			start.HiLite = true;
			end.HiLite = true;
			//return;

			Stack<CNode> stack = new Stack<CNode>();
			//stack.Push(start);
			//_search(start, start, stack, end);

			//_search2(start, end);
			CRoute r= _search4(start, end);
			Console.WriteLine("ROUTE {0}",r.Count);


			return;

			int r1 = 0;
			int r2 = 100000000;

			while (r2 != r1)
			{
				r2 = r1;
				r1 = _search2(start, end);
				Console.WriteLine("-- {0} {1}", r1, r2);
			}



		}


		public List<CNode> GetNeighbors(CNode node)
		{
			List<CNode> ret = new List<CNode>();
			List<CEdge> edges = GetEdges(node);
			foreach (CEdge e in edges)
			{
				if (e.StartNode != node)
				{
					ret.Add(e.StartNode);
				}
				else
				{
					ret.Add(e.EndNode);
				}



			}
			return ret;
		}


		private CRoute _search4(CNode start, CNode target)
		{
			start.Name = "START";
			target.Name = "TARGET";
			List<CNode> closedSet = new List<CNode>();
			List<CNode> openSet = new List<CNode>();
			Dictionary<CNode, double> g_scores = new Dictionary<CNode, double>();
			Dictionary<CNode, double> f_scores = new Dictionary<CNode, double>();
			Dictionary<CNode, double> h_scores = new Dictionary<CNode, double>();
			Dictionary<CNode, CNode> came_from = new Dictionary<CNode, CNode>();

			openSet.Add(start);
			g_scores [start] = 0;

			while (openSet.Count > 0)
			{
				CNode xNode = null;
				double fscore = Double.PositiveInfinity;
				foreach (CNode nNode in openSet)
				{
					if (!f_scores.ContainsKey(nNode))
					{
						f_scores.Add(nNode, 0);
					}
					if (fscore > f_scores [nNode])
					{
						fscore = Math.Min(f_scores [nNode], fscore);
						xNode = nNode;
					}
				}
				if (xNode == target)
				{
					CNode tmp = target;
					CRoute llist = new CRoute();
					while (came_from.ContainsKey(tmp))
					{
						if (came_from [tmp] != null)
						{
							llist.AddLast(GetEdge(came_from [tmp], tmp, true));
						}
						tmp = came_from [tmp];
					}
					return llist;
				}

				openSet.Remove(xNode);
				closedSet.Add(xNode);

				List<CNode> neighbors_x = GetNeighbors(xNode);
				foreach (CNode yNode in neighbors_x)
				{
					if (closedSet.Contains(yNode))
					{
						continue;
					}
					double tg_score = g_scores [xNode] + dist(xNode.Location, yNode.Location);
					bool t_isbetter = false;
					if (!openSet.Contains(yNode))
					{
						openSet.Add(yNode);
						h_scores [yNode] = dist(yNode.Location, target.Location);// := heuristic_estimate_of_distance_to_goal_from(yNode)
						t_isbetter = true;
					}
					else if (tg_score < g_scores [yNode])
					{
						t_isbetter = true;
					}
					if (t_isbetter)
					{
						came_from [yNode] = xNode;
						g_scores [yNode] = tg_score;
						f_scores [yNode] = tg_score + h_scores [yNode];
					}
				}
			}
			return null;
		}



		private CRoute _search3(CNode start, CNode target)
		{
			start.Name = "START";
			target.Name = "TARGET";
			List<CNode> closedSet = new List<CNode>();
			List<CNode> openSet = new List<CNode>();
			Dictionary<CNode, double> g_scores = new Dictionary<CNode, double>();
			Dictionary<CNode, double> f_scores = new Dictionary<CNode, double>();
			Dictionary<CNode, double> h_scores = new Dictionary<CNode, double>();
			Dictionary<CNode, CNode> came_from = new Dictionary<CNode, CNode>();

			openSet.Add(start);
			g_scores [start] = 0;

			while (openSet.Count > 0)
			{
				CNode x = null;
				double fscore = Double.PositiveInfinity;
				foreach (CNode n in openSet)
				{
					if (!f_scores.ContainsKey(n))
					{
						f_scores.Add(n, 0);
					}
					if (fscore > f_scores [n])
					{
						fscore = Math.Min(f_scores [n], fscore);
						x = n;
					}
				}
				if (x == target)
				{
					//Console.WriteLine(came_from[start]==null);
					CNode tmp = target;
					CRoute llist = new CRoute();
					//llist.AddLast(tmp);

					//Console.WriteLine("**{0} -> {1}", tmp.Name, came_from [tmp].Name);
					while (came_from.ContainsKey(tmp))
					{
						if (came_from [tmp] != null)
						{
							llist.AddLast(GetEdge(came_from [tmp], tmp, true));
						}
						tmp = came_from [tmp];
					}


					return llist;
				}

				openSet.Remove(x);
				closedSet.Add(x);

				List<CNode> neighbors_x = GetNeighbors(x);
				foreach (CNode y in neighbors_x)
				{
					if (closedSet.Contains(y))
						continue;
					double tg_score = g_scores [x] + dist(x.Location, y.Location);
					bool t_isbetter = false;
					if (!openSet.Contains(y))
					{
						openSet.Add(y);
						h_scores [y] = dist(y.Location, target.Location);// := heuristic_estimate_of_distance_to_goal_from(yNode)
						t_isbetter = true;
					}
					else if (tg_score < g_scores [y])
					{
						t_isbetter = true;
					}
					if (t_isbetter)
					{

						came_from [y] = x;
						g_scores [y] = tg_score;
						f_scores [y] = tg_score + h_scores [y];//Estimated total distance from start to goal through yNode.
					}
				}
			}
			return null;

		}



		private double dist(PointF p1, PointF p2)
		{
			return Math.Pow(p1.X - p2.X, 2) + Math.Pow(p1.Y - p2.Y, 2);
		}

		private int _search2(CNode start, CNode target)
		{
			Stack<CNode> stack = new Stack<CNode>();
			CNode cNode = start;

			int k = 0;
			while (cNode != target)
			{
				CNode nextNode = null;
				List<CNode> adj = GetAdjacent(cNode);
				/*List<CNode> adj2 = new List<CNode>();

				foreach (CNode next in adj)
				{
					if (next.Mark || stack.Contains(next))
					{
						continue;
					}
					adj2.Add(next);
				}
				*/

				double mind = Double.PositiveInfinity;
				CNode minNext = null;

				foreach (CNode next in adj)
				{
					if (next.Mark || stack.Contains(next))
					{
						continue;
					}
					if (dist(next.Location, target.Location) < mind)
					{
						mind = dist(next.Location, target.Location);
						minNext = next;
					}
				}
				nextNode = minNext;
				if (nextNode == null)
				{
					cNode.Mark = true;
					if (stack.Count > 0)
						cNode = stack.Pop();
				}
				else
				{
					stack.Push(cNode);
					cNode = nextNode;
				}

			}




			int ret = stack.Count;

			CNode n1 = target;
			CNode n2 = null;
			while (stack.Count > 0)
			{

				//stack.Pop().HiLite = true;
				n2 = n1;
				n1 = stack.Pop();
				CEdge ed1 = GetEdge(n1, n2);
				CEdge ed2 = GetEdge(n2, n1);
				if (n1 != null)
					n1.HiLite = true;
				if (n2 != null)
					n2.HiLite = true;
				if (ed1 != null)
					ed1.HiLite = true;
				if (ed2 != null)
					ed2.HiLite = true;

				Console.WriteLine("STACK {0}", n1.Name);
			}

			return ret;

		}








		private Stack<CNode> shortest = null;
		bool reset = false;


		private void _search(CNode start0, CNode start, Stack<CNode> stack, CNode target)
		{

			if (reset)
			{

				/*start = start0;
				reset = false;
				_search(start0, start0, stack, target);*/


				/*while (stack.Count > 0)
				{
					stack.Pop().HiLite = true;
				}
				 * */

				stack.Clear();
				return;
			}

			List<CEdge> eds = this.GetEdges(start, CNodeRole.Start);
			stack.Push(start);


			if (eds.Count == 1)
			{
				stack.Pop().Mark = true;
			}
			foreach (CEdge e in eds)
			{
				if (e.EndNode.Mark || e.EndNode == stack.Peek())
				{
					continue;
				}

				if (e.EndNode == target)
				{
					Console.WriteLine("FOUND");
					if (shortest == null || (shortest != null && shortest.Count >= stack.Count))
					{

						shortest = stack;
						stack = new Stack<CNode>();
						reset = true;
						if (shortest.Count == stack.Count)
						{
							Console.WriteLine("SAME SOL?");
						}


						Console.WriteLine("RESET {0}", shortest.Count);
					}
					return;
				}
				_search(start0, e.EndNode, stack, target);
			}


		}














	}
}
